#include <iostream>

#include <opencv2/opencv.hpp>
#include <fstream>


using namespace std;
using namespace cv;

int img_cols = 1280;  //摄像头的分辨率
int img_rows = 720;
Size imageSize = Size(img_cols, img_rows);

Rect validROIL, validROIR;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域, 其内部的所有像素都有效

Mat mapLx, mapLy, mapRx, mapRy;
Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R，投影矩阵P 重投影矩阵Q

Mat cameraMatrixL, cameraMatrixR, distCoeffL, distCoeffR;
// Mat cameraMatrixL = (Mat_<double>(3, 3) << 458.654, 0.0, 367.215, 0.0, 457.296, 248.375,
//         0.0, 0.0, 1.0);
// Mat distCoeffL = (Mat_<double>(5, 1) << -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05,0);

// Mat cameraMatrixR = (Mat_<double>(3, 3) << 457.587, 0.0, 379.999, 0.0, 456.134, 255.238,
//         0.0, 0.0, 1.0);
// Mat distCoeffR = (Mat_<double>(5, 1) << -0.28368365, 0.07451284, -0.00010473, -3.55590700e-05,0);

//左右目之间的R,t可通过stereoCalibrate()或matlab工具箱calib求得
Mat trans, R;
// Mat trans = (Mat_<double>(3, 1) << -59.49161163342028, 0.010760053363461395, -1.2873374693176298);//T平移向量
// Mat R = (Mat_<double>(3, 3) << 0.9999522267540742, -6.925008919407085E-4, -0.009750110362602303,
//         6.897833651297106E-4, 0.9999997223155306, -2.820775973100296E-4,
//         0.009750302994135816, 2.753386576112956E-4, 0.9999524267584664);;//R 旋转矩阵

bool loadAndWriteCamInfo(std::string filename)
{
    // Load settings related to stereo calibration
    cv::FileStorage fsSettings(filename, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        cerr << "ERROR: Wrong path to settings" << endl;
        return -1;
    }

    img_cols = fsSettings["width"];
    img_rows = fsSettings["height"];
    imageSize = Size(img_cols, img_rows);

    fsSettings["K1"] >> cameraMatrixL;
    fsSettings["K2"] >> cameraMatrixR;
    fsSettings["D1"] >> distCoeffL;
    fsSettings["D2"] >> distCoeffR;

    fsSettings["rot"] >> R;
    fsSettings["trans"] >> trans;

    cout << "width: " << img_cols << endl
         << "height: " << img_rows << endl
         << "K1: " << cameraMatrixL << endl
         << "K2: " << cameraMatrixR << endl
         << "D1: " << distCoeffL << endl
         << "D2: " << distCoeffR << endl
         << "rot: " << R << endl
         << "trans: " << trans << endl;

    /// 3、计算去畸变参数
////            //    Rodrigues(rec, R); //Rodrigues变换
////            //经过双目标定得到摄像头的各项参数后，采用OpenCV中的stereoRectify(立体校正)得到校正旋转矩阵R、投影矩阵P、重投影矩阵Q
////            //flags-可选的标志有两种零或者 CV_CALIB_ZERO_DISPARITY ,如果设置 CV_CALIB_ZERO_DISPARITY 的话，该函数会让两幅校正后的图像的主点有相同的像素坐标。否则该函数会水平或垂直的移动图像，以使得其有用的范围最大
////            //alpha-拉伸参数。如果设置为负或忽略，将不进行拉伸。如果设置为0，那么校正后图像只有有效的部分会被显示（没有黑色的部分），如果设置为1，那么就会显示整个图像。设置为0~1之间的某个值，其效果也居于两者之间。
    stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, trans, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, 0, imageSize, &validROIL, &validROIR);
    // //再采用映射变换计算函数initUndistortRectifyMap得出校准映射参数,该函数功能是计算畸变矫正和立体校正的映射变换
    // initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
    // initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
    // 基于上面计算得到的映射变换参数，得到校正后的图像
    // cv::remap(left_img,left_img,mapLx,mapLy,cv::INTER_LINEAR);
    // cv::remap(right_img,right_img,mapRx,mapRy,cv::INTER_LINEAR);

    cout << "Rl: \n" << Rl << endl;
    cout << "Rr: \n" << Rr << endl;
    cout << "Pl: \n" << Pl << endl;
    cout << "Pr: \n" << Pr << endl;
    cout << "Q: \n" << Q << endl;
    cout << "validROIL: \n" << validROIL << endl;
    cout << "validROIR: \n" << validROIR << endl;


    ofstream file_left_info,file_right_info;
    file_left_info.open("left.yaml", ios::out | ios::trunc );
    file_right_info.open("right.yaml", ios::out | ios::trunc );
    file_left_info << "%YAML:1.0" << endl;
    file_left_info << "image_width: "  << img_cols << endl;
    file_left_info << "image_height: "  << img_rows << endl;
    file_left_info << "camera_name: "  << "my_stereo" << endl;
    file_left_info << "distortion_model: "  << "plumb_bob" << endl;
    file_left_info << "camera_matrix: " <<endl
                   << "  rows: " << "3" <<endl
                   << "  cols: " << "3" <<endl
                   << "  dt: " << "d" << endl
                   << "  data: " << cameraMatrixL.reshape(1,1)  <<endl;
    file_left_info << "distortion_coefficients: "  << endl
                   << "  rows: " << "1" <<endl
                   << "  cols: " << distCoeffL.size[1] <<endl
                   << "  dt: " << "d" << endl
                   << "  data: " << distCoeffL  <<endl;
    file_left_info << "rectification_matrix: "   << endl
                   << "  rows: " << "3" <<endl
                   << "  cols: " << "3" <<endl
                   << "  dt: " << "d" << endl
                   << "  data: " << Rl.reshape(1,1)  <<endl;

    file_left_info << "projection_matrix: "  << endl
                   << "  rows: " << "3" <<endl
                   << "  cols: " << "4" <<endl
                   << "  dt: " << "d" << endl
                   << "  data: " << Pl.reshape(1,1)  <<endl;

    file_right_info << "%YAML:1.0" << endl;
    file_right_info << "image_width: "  << img_cols << endl;
    file_right_info << "image_height: "  << img_rows << endl;
    file_right_info << "camera_name: "  << "my_stereo" << endl;
    file_right_info << "distortion_model: "  << "plumb_bob" << endl;
    file_right_info << "camera_matrix: " <<endl
                   << "  rows: " << "3" <<endl
                   << "  cols: " << "3" <<endl
                   << "  dt: " << "d" << endl
                   << "  data: " << cameraMatrixR.reshape(1,1)  <<endl;
    file_right_info << "distortion_coefficients: " << endl
                   << "  rows: " << "1" <<endl
                   << "  cols: " << distCoeffR.size[1] <<endl
                   << "  dt: " << "d" << endl
                   << "  data: " << distCoeffR  <<endl;
    file_right_info << "rectification_matrix: "   << endl
                   << "  rows: " << "3" <<endl
                   << "  cols: " << "3" <<endl
                   << "  dt: " << "d" << endl
                   << "  data: " << Rr.reshape(1,1)  <<endl;
    file_right_info << "projection_matrix: "  << endl
                   << "  rows: " << "3" <<endl
                   << "  cols: " << "4" <<endl
                   << "  dt: " << "d" << endl
                   << "  data: " << Pr.reshape(1,1)  <<endl;


    file_left_info.close();
    file_right_info.close();

    fsSettings.release();


    return true;
}

int main(int argc, char* argv[])
{
    //-- Zuo丶    Load Camera YAML
    loadAndWriteCamInfo("../mystereo.yaml");
}